package javavis.jip3d.functions;

import java.util.ArrayList;

import javax.vecmath.Color3f;

import cern.colt.matrix.DoubleMatrix2D;
import cern.colt.matrix.impl.DenseDoubleMatrix2D;
import cern.colt.matrix.linalg.Algebra;
import cern.colt.matrix.linalg.SingularValueDecomposition;

import javavis.base.Function3DGroup;
import javavis.base.JIPException;
import javavis.base.ParamType;
import javavis.jip3d.geom.MyTransform3D;
import javavis.jip3d.geom.Normal3D;
import javavis.jip3d.geom.Plane3D;
import javavis.jip3d.geom.Point3D;
import javavis.jip3d.geom.Segment3D;
import javavis.jip3d.geom.Vector3D;
import javavis.jip3d.gui.Function3D;
import javavis.jip3d.gui.FunctionParam;
import javavis.jip3d.gui.ScreenData;
import javavis.jip3d.gui.ScreenOptions;
import javavis.jip3d.gui.dataobjects.PlaneSet3D;
import javavis.jip3d.gui.dataobjects.SegmentSet3D;
/**
 * 
 * @author dviejo
 *
 */
public class FEgomotion3D extends Function3D {
	/**
	 * @uml.property  name="alpha"
	 */
	private double alpha;

	/**
	 * @uml.property  name="transform"
	 * @uml.associationEnd  
	 */
	private MyTransform3D transform;
	/**
	 * @uml.property  name="error"
	 */
	private double error;

	public FEgomotion3D() {
		super();
		this.allowed_input = ScreenOptions.tPLANARSET3D;
		this.group = Function3DGroup.Egomotion;


		transform = null;
		error = Double.MAX_VALUE;
		alpha = 0.025;

		FunctionParam p1 = new FunctionParam("Model", ParamType.SCRDATA);
		FunctionParam p2 = new FunctionParam("Alpha", ParamType.FLOAT);
		p2.setValue(0.15);
		FunctionParam p3 = new FunctionParam("Iterations", ParamType.INT);
		p3.setValue(75);
		FunctionParam p4 = new FunctionParam("Intermediate", ParamType.BOOL);
		p4.setValue(false);
		FunctionParam p5 = new FunctionParam("Verbose", ParamType.BOOL);
		p5.setValue(true);

		this.addParam(p1);
		this.addParam(p2);
		this.addParam(p3);
		this.addParam(p4);
		this.addParam(p5);
	}

	@Override
	public void proccessData(ScreenData scene) throws JIPException {
		result_list = new ArrayList<ScreenData>();
		ScreenData model = this.paramValueScrData("Model");
		alpha = this.paramValueReal("Alpha");
		int iterations = this.paramValueInt("Iterations");
		int cont;
		double prev_error;
		MyTransform3D tr3d = new MyTransform3D();
		Object []elements = model.elements();
		Object []model_tr;
		double []translation;
		boolean intermediate = this.paramValueBool("Intermediate");
		boolean verbose = paramValueBool("Verbose");

		ArrayList<Pair> pairs;

		double prog_inc = 50.0/iterations;

		cont = 0;
		do
		{
			progress += prog_inc;
			cont++;
			prev_error = error;
			model_tr = applyTransform(elements, tr3d);
			pairs = findPairs(elements, model_tr, scene.elements(), cont);

			if(intermediate)
				mostrarPares(pairs, cont);

			tr3d = computeRotation(pairs);
			error = tr3d.getAngX()*tr3d.getAngX() + tr3d.getAngY()*tr3d.getAngY() + tr3d.getAngZ()*tr3d.getAngZ();
		} while(cont<iterations && Math.abs(error-prev_error)>0.0002);
		if(verbose) System.out.print("Iteraciones: "+cont);

		//TODO es posible que esto lo tengamos que meter en un bucle tipo ICP
		//rotate model patches
		progress = 50;
		Object []rotated_elements = applyTransform(elements, tr3d);
		MyTransform3D tr3d_new = new MyTransform3D();
		MyTransform3D tr3d_inc = new MyTransform3D();;
		cont = 0;
		error = Double.MAX_VALUE;
		do
		{
			cont++;
			progress += prog_inc;
			prev_error = error;
			model_tr = applyTransform(rotated_elements, tr3d_new);
			//	recompute pairs but with rotated model patches
			pairs = findPairsTrans(model_tr, model_tr, scene.elements(), cont);// 3);//cont);
			//compute translation
			translation = computeTranslation(pairs);
			if(intermediate)
			{
				mostrarPares(pairs, cont);
				System.out.println("partial tr: "+translation[0]+", "+translation[1]+", "+translation[2]);
			}
			tr3d_inc.setTranslation(translation);
			tr3d_new.applyTransform(tr3d_inc);
			error = (translation[0]*translation[0] + translation[1]*translation[1] + translation[2]*translation[2]);
		} while(cont<iterations && error>0.00025);
		if(verbose) System.out.println(", "+cont);
		tr3d.setTranslation(tr3d_new.getTranslation());
		model_tr = applyTransform(elements, tr3d);

		transform = tr3d;

		PlaneSet3D resulttras = new PlaneSet3D(new ScreenOptions());
		resulttras.name = "egomotion3D";
		resulttras.scr_opt.width = 1;
		resulttras.scr_opt.color = new Color3f(0,1,0);
		resulttras.scr_opt.global_color = true;
		for(cont=0;cont<model_tr.length;cont++)
			resulttras.insert((Plane3D)model_tr[cont]);
		result_list.add(resulttras);
		if(verbose) System.out.println("T: "+tr3d.toString());

	}

	public ArrayList<Pair> findPairs(Object []model, Object []model_tr, Object []scene, int iteration)
	{
		ArrayList<Pair> ret = new ArrayList<Pair>();
		Plane3D p_model;
		Plane3D p_scene;
		int cont;
		double dist;
		double weight;
		double aux;
		double media, varianza;

		media = 0;
		for(cont=0;cont<model_tr.length;cont++)
		{
			p_model = (Plane3D)model_tr[cont];
			p_scene = findClosest(p_model, scene);
//			dist = calcDist(p_model, p_scene);
			if(p_scene.getAngle(p_model)<0.9)
			{
				dist = p_scene.getAngle(p_model);
				media += dist;
				ret.add(new Pair((Plane3D)model[cont], p_scene, dist));
			}
		}

		int tam = ret.size();
		media /= tam;
		varianza = 0.0000001;
		for(cont=0;cont<tam;cont++)
		{
			aux = ret.get(cont).distance - media;
			varianza += aux * aux;
		}
		varianza /= tam;
		varianza *= 2;
		double normalization = 2* Math.PI * varianza;
//		varianza /= iteration;
		for(cont=0;cont<tam;cont++)
		{
			dist = ret.get(cont).distance - media;
			weight = Math.exp(-(dist*dist)/(varianza)) / normalization;
			ret.get(cont).distance = weight;
		}


		return ret;
	}

	public ArrayList<Pair> findPairsTrans(Object []model, Object []model_tr, Object []scene, int iteration)
	{
		ArrayList<Pair> ret = new ArrayList<Pair>();
		Plane3D p_model;
		Plane3D p_scene;
		int cont;
		double dist;
		double weight;
		double aux;
		double media, varianza;

		media = 0;
		for(cont=0;cont<model_tr.length;cont++)
		{
			p_model = (Plane3D)model_tr[cont];
			p_scene = findClosestTrans(p_model, scene);
			if(Math.abs(p_model.anglePlane(p_scene))<0.2)
			{
//				dist = calcDist2(p_model, p_scene);
				dist = Math.abs(p_scene.pointDistance(p_model.origin)) + p_scene.getAngle(p_model);
				media += dist;
				ret.add(new Pair((Plane3D)model[cont], p_scene, dist));
			}
		}

		int tam = ret.size();
		media /= tam;
		varianza = 0.0000001;
		for(cont=0;cont<tam;cont++)
		{
			aux = ret.get(cont).distance - media;
			varianza += aux * aux;
		}
		varianza /= tam;
		varianza *= 2;
		double normalization = 1; //2* Math.PI * varianza;
//		varianza /= iteration;
		for(cont=0;cont<tam;cont++)
		{
			dist = ret.get(cont).distance - media;
			//esta comprobacion es solo para pruebas sinteticas
			if(varianza>0)
				weight = Math.exp(-(dist*dist)/(varianza)) / normalization;
			else weight = 1;

			ret.get(cont).distance = weight;
		}
		return ret;
	}


	/**
	 * This method visits all the Plane3D objects from an array and retrieves who that
	 * have minimum distance to p_scene planar patch
	 * @param p_model
	 * @param scene
	 * @return
	 */
	public Plane3D findClosest(Plane3D p_model, Object[]scene)
	{
		Plane3D ret = null;
		double best_dist = Double.MAX_VALUE;
		double dist;
		Plane3D p_scene;
		int cont;

		for(cont=0;cont<scene.length;cont++)
		{
			p_scene = (Plane3D)scene[cont];
			dist = calcDist(p_model, p_scene);
			if(dist<best_dist)
			{
				best_dist = dist;
				ret = p_scene;
			}
		}

		return ret;
	}

	/**
	 * This method visits all the Plane3D objects from an array and retrieves who that
	 * have minimum distance to p_scene planar patch
	 * @param p_model
	 * @param scene
	 * @return
	 */
	public Plane3D findClosestTrans(Plane3D p_model, Object[]scene)
	{
		Plane3D ret = null;
		double best_dist = Double.MAX_VALUE;
		double dist;
		Plane3D p_scene;
		int cont;

		for(cont=0;cont<scene.length;cont++)
		{
			p_scene = (Plane3D)scene[cont];
			dist = calcDist2(p_model, p_scene);
			if(dist<best_dist)
			{
				best_dist = dist;
				ret = p_scene;
			}
		}

//		error += best_dist;

		return ret;
	}

	//por ahora descarto esta opcion por tener que resolver la correspondencia entra puntos de 2 arboles kd
//	public Plane3D findClosest(Plane3D p_scene, arbolKD model)
//	{
//		Plane3D ret = null;
//		double []min_range = p_scene.origin.getCoords();
//		double []max_range = p_scene.origin.getCoords();
//		double best_dist, dist;
//		Object []elements;
//		Plane3D p_model;
//		int cont;
//
//		//get nearest point (euclidean distance)
//		try
//		{
//			ret = (Plane3D)model.nearest(p_scene.origin.getCoords());
//
//			//compute its distance (our new distance definition)
//			best_dist = calcDist(p_scene, ret);
//			for(cont=0;cont<3;cont++)
//			{
//				min_range[cont] -= best_dist;
//				max_range[cont] += best_dist;
//			}
//
//			elements = model.range(min_range, max_range);
//			for(cont=0;cont<elements.length;cont++)
//			{
//				p_model = (Plane3D)elements[cont];
//				dist = calcDist(p_scene, p_model);
//				if(dist<best_dist)
//				{
//					best_dist = dist;
//					ret = p_model;
//				}
//			}
//		}catch (Exception e){}
//
//		return ret;
//	}

	private double calcDist(Plane3D p_scene, Plane3D p_model)
	{
		double angle = p_scene.getAngle(p_model);
		double ret = p_scene.origin.getDistance(p_model.origin);
//		if(ret!=0)
//			ret += (1.0 / ret) * angle;
//		else
//			ret = angle;
		ret *= alpha; //0.025 funciona en poli
		ret += angle;
		return ret;
	}

	private double calcDist2(Plane3D p_scene, Plane3D p_model)
	{
//		double ret;
		double angle = p_scene.getAngle(p_model);
		double dist = p_scene.origin.getDistance(p_model.origin);
//		dist = Math.abs(p_scene.pointDistance(p_model.origin));

//		ret = dist;
//		dist /= 4;
//		ret += angle/dist; //*0.03;
//		return ret;
		return dist + angle;
	}


	public MyTransform3D computeRotation(ArrayList<Pair> pairs)
	{
		MyTransform3D ret = null;
		double [][]matrix = new double [3][3];
		DenseDoubleMatrix2D ddmatrix;
		DoubleMatrix2D U;
		SingularValueDecomposition SVD;

		matrix[0][0] = matrix[0][1] = matrix[0][2] =
			matrix[1][0] = matrix[1][1] = matrix[1][2] =
			matrix[2][0] = matrix[2][1] = matrix[2][2] = 0;

		for(Pair p: pairs)
		{
			matrix[0][0] += p.distance * p.model.vector.getX() * p.scene.vector.getX();
			matrix[0][1] += p.distance * p.model.vector.getX() * p.scene.vector.getY();
			matrix[0][2] += p.distance * p.model.vector.getX() * p.scene.vector.getZ();

			matrix[1][0] += p.distance * p.model.vector.getY() * p.scene.vector.getX();
			matrix[1][1] += p.distance * p.model.vector.getY() * p.scene.vector.getY();
			matrix[1][2] += p.distance * p.model.vector.getY() * p.scene.vector.getZ();

			matrix[2][0] += p.distance * p.model.vector.getZ() * p.scene.vector.getX();
			matrix[2][1] += p.distance * p.model.vector.getZ() * p.scene.vector.getY();
			matrix[2][2] += p.distance * p.model.vector.getZ() * p.scene.vector.getZ();
		}

		ddmatrix = new DenseDoubleMatrix2D(matrix);
		SVD = new SingularValueDecomposition(ddmatrix);

		// Calculo la matriz U y V
		DenseDoubleMatrix2D UT, V, X;
		X=new DenseDoubleMatrix2D(3,3);

		U=(DenseDoubleMatrix2D)SVD.getU();
		V=(DenseDoubleMatrix2D)SVD.getV();
		UT=(DenseDoubleMatrix2D)U.viewDice(); // Calcula la transpuesta
		V.zMult(UT,X);

		Algebra algebra = new Algebra();
//System.out.println("det: "+algebra.det(X));
		if (algebra.det(X)< 0) { //-0.9
			V.setQuick(0,2,-V.getQuick(0,2));
			V.setQuick(1,2,-V.getQuick(1,2));
			V.setQuick(2,2,-V.getQuick(2,2));
			V.zMult(UT,X);
		}

		ret = new MyTransform3D(X);
		return ret;
	}

	private Vector3D []findMainDirections(Vector3D []vectors) //ArrayList<Pair> pairs)
	{
		Vector3D []ret = new Vector3D[3];
    	int cont;
		double [][]matriz = new double [3][3];
		DenseDoubleMatrix2D matrix;
		DoubleMatrix2D U;
		SingularValueDecomposition SVD;
		double []datos;

		matriz[0][0] = matriz[0][1] = matriz[0][2] =
			matriz[1][0] = matriz[1][1] = matriz[1][2] =
			matriz[2][0] = matriz[2][1] = matriz[2][2] = 0;

		for(cont=0;cont<vectors.length;cont++)
		{
			datos = vectors[cont].getCoords();
			matriz[0][0] += datos[0] * datos[0];
			matriz[0][1] += datos[0] * datos[1];
			matriz[0][2] += datos[0] * datos[2];
			matriz[1][1] += datos[1] * datos[1];
			matriz[1][2] += datos[1] * datos[2];
			matriz[2][2] += datos[2] * datos[2];
		}
		matriz[1][0] = matriz[0][1];
		matriz[2][0] = matriz[0][2];
		matriz[2][1] = matriz[1][2];

		matrix = new DenseDoubleMatrix2D(matriz);
		SVD = new SingularValueDecomposition(matrix);
		U = SVD.getU();
		ret[0] = new Vector3D(U.getQuick(0, 0), U.getQuick(1, 0), U.getQuick(2, 0));
		ret[1] = new Vector3D(U.getQuick(0, 1), U.getQuick(1, 1), U.getQuick(2, 1));
		ret[2] = new Vector3D(U.getQuick(0, 2), U.getQuick(1, 2), U.getQuick(2, 2));

		return ret;
	}

	public double[] computeTranslation(ArrayList<Pair> pairs)
	{
		double []ret = new double[3];
		int tam = pairs.size();
		Vector3D v;
		Vector3D []vectors = new Vector3D[tam]; //translation vectors
		double []norms = new double[tam];
		double cos_angle;
		double total1, total2, total3;
		int cont;
		Point3D projection;
		double []contribution = new double[3];

		cont = 0;
		for(Pair p:pairs)
		{
			projection = p.scene.pointProjection(p.model.origin);
			v = new Vector3D(projection.subPoint(p.model.origin));
			norms[cont] = v.module;
			v.normalize();
			vectors[cont] = v;
			cont++;
		}
		Vector3D []main_directions = findMainDirections(vectors);
//System.out.println(main_directions[0].toString());
//System.out.println(main_directions[1].toString());
//System.out.println(main_directions[2].toString());
//System.out.println("---------------------------------------------");
		total1 = total2 = total3 = 0;
		contribution[0] = contribution[1] = contribution[2] = 0;
		cont = 0;
		for(Pair p:pairs)
		{
			//main direction #1
			cos_angle = main_directions[0].dotProduct(vectors[cont]);
			contribution[0] += norms[cont] * cos_angle * p.distance;
			total1 += p.distance * Math.abs(cos_angle);

			//main direction #2
			cos_angle = main_directions[1].dotProduct(vectors[cont]);
			contribution[1] += norms[cont] * cos_angle * p.distance;
			total2 += p.distance * Math.abs(cos_angle);

			//main direction #3
			cos_angle = main_directions[2].dotProduct(vectors[cont]);
			contribution[2] += norms[cont] * cos_angle * p.distance;
			total3 += p.distance * Math.abs(cos_angle);

			cont++;
		}
		if(total1>0)
			contribution[0] /= total1;
		else contribution[0] = 0;
		if(total2>0)
			contribution[1] /= total2;
		else contribution[1] = 0;
		if(total3>0)
			contribution[2] /= total3;
		else contribution[2] = 0;

		ret[0] = main_directions[0].getX() * contribution[0] +
			main_directions[1].getX() * contribution[1] + main_directions[2].getX() * contribution[2];
		ret[1] = main_directions[0].getY() * contribution[0] +
			main_directions[1].getY() * contribution[1] + main_directions[2].getY() * contribution[2];
		ret[2] = main_directions[0].getZ() * contribution[0] +
			main_directions[1].getZ() * contribution[1] + main_directions[2].getZ() * contribution[2];

		return ret;
	}

	/** This function computes the traslation vector for alignning two set of matched patches like
	 * a ponderate mean of pair-wise match translation vectors.
	 * @param pairs have the information of matched planar patches
	 * @return three-coordinates of translation vector
	 */
	public double [] computeTranslation2(ArrayList<Pair> pairs)
	{
		double []ret = new double[3];
		double total;
		Vector3D v;
		Point3D projection;
		double contribution;

		ret[0] = ret[1] = ret[2] = 0;
		total = 0;

		for(Pair p: pairs)
		{
			projection = p.scene.pointProjection(p.model.origin);
			v = new Vector3D(projection.subPoint(p.model.origin));
			contribution = p.distance;
			ret[0] += v.getX() * contribution;
			ret[1] += v.getY() * contribution;
			ret[2] += v.getZ() * contribution;
			total +=  contribution;
		}
		if(total>0)
		{
			ret[0] /= total;
			ret[1] /= total;
			ret[2] /= total;
		}

		return ret;
	}

	/** This function computes the translation vector for aligning two set of matched patches like
	 * the vector between the center of mass of the two set of patches that are involved into the matches.
	 * @param pairs have the information of matched planar patches
	 * @return three-coordinates of translation vector
	 */
	public double [] computeTranslation3(ArrayList<Pair> pairs)
	{
		double []centre1 = new double[3];
		double []centre2 = new double[3];
		double total;
		Vector3D v;
		Point3D cm1;
		double contribution;

		centre1[0] = centre1[1] = centre1[2] = 0;
		centre2[0] = centre2[1] = centre2[2] = 0;
		total = 0;

		for(Pair p: pairs)
		{
			contribution = p.distance;
			centre1[0] += p.scene.origin.getX() * contribution;
			centre1[1] += p.scene.origin.getY() * contribution;
			centre1[2] += p.scene.origin.getZ() * contribution;
			centre2[0] += p.model.origin.getX() * contribution;
			centre2[1] += p.model.origin.getY() * contribution;
			centre2[2] += p.model.origin.getZ() * contribution;
			total +=  contribution;
		}

		if(total>0)
		{
			centre1[0] /= total;
			centre1[1] /= total;
			centre1[2] /= total;
			centre2[0] /= total;
			centre2[1] /= total;
			centre2[2] /= total;
		}

		cm1 = new Point3D(centre1);
		v = new Vector3D(cm1.subPoint(new Point3D(centre2)));
		return v.getCoords();
	}


	/**
	 * @author  miguel
	 */
	public class Pair
	{
		/**
		 * @uml.property  name="model"
		 * @uml.associationEnd  
		 */
		public Plane3D model;
		/**
		 * @uml.property  name="scene"
		 * @uml.associationEnd  
		 */
		public Plane3D scene;
		public double distance;

		public Pair()
		{
			model = null;
			scene = null;
			distance = 0.0;
		}

		public Pair(Plane3D m, Plane3D s, double d)
		{
			model = m;
			scene = s;
			distance = d;
		}
	}

	public Object[] applyTransform(Object[] scene, MyTransform3D tr3d)
	{
		Plane3D plane;
		int cont;
		Object []ret = new Object[scene.length];

		for(cont=0;cont<scene.length;cont++)
		{
			plane = new Plane3D((Plane3D)scene[cont]);
			plane.applyTransform(tr3d);
			ret[cont] = plane;
		}
		return ret;
	}

	/**
	 * @return
	 * @uml.property  name="transform"
	 */
	public MyTransform3D getTransform()
	{
		return transform;
	}

    public Plane3D[] subsMean (Object []set, Point3D mean) {
    	Plane3D []setR=new Plane3D[set.length];
    	Plane3D plane;
    	Point3D origin;
    	for (int i=0; i<set.length; i++) {
    		plane = (Plane3D)set[i];
    		origin = plane.origin.subPoint(mean);
    		setR[i] = new Plane3D(new Normal3D(origin,plane.vector));
    	}
    	return setR;
    }

    public Point3D calcMean (Object []set) {
    	double x, y, z;
    	Plane3D plane;

    	x=0.0;
		y=0.0;
		z=0.0;
    	for (int i=0; i<set.length; i++) {
    		plane = (Plane3D)set[i];
    		x += plane.origin.getX();
			y += plane.origin.getY();
			z += plane.origin.getZ();
    	}
    	x /= set.length;
		y /= set.length;
		z /= set.length;

    	Point3D paux = new Point3D(x, y, z);
    	return paux;
    }

    private void mostrarPares(ArrayList<Pair> pairs, int cont)
    {
    	SegmentSet3D pares;
    	SegmentSet3D mov;
    	PlaneSet3D pset;
    	pares = new SegmentSet3D(new ScreenOptions());
    	mov = new SegmentSet3D(new ScreenOptions());
    	pset = new PlaneSet3D(new ScreenOptions());
    	pares.name = "pairs"+cont;
    	pares.scr_opt.width = 1;
    	pares.scr_opt.color = new Color3f(1,0,0);
    	mov.name = "mov"+cont;
    	mov.scr_opt.width = 1;
    	mov.scr_opt.color = new Color3f(0,0.5f,1);
    	pset.name = "patches"+cont;
    	pset.scr_opt.width = 1;
    	pset.scr_opt.color = new Color3f(1,0.5f,0);
    	for(Pair p: pairs)
    	{
    		pares.insert(new Segment3D(new Point3D(p.model.origin), p.scene.origin));
    		mov.insert(new Segment3D(new Point3D(p.model.origin), p.scene.pointProjection(p.model.origin)));
    		pset.insert(new Plane3D(p.model));
    	}
    	result_list.add(pares);
    	result_list.add(mov);
    	result_list.add(pset);

    }

}
